package toolib.geom;

public class Intersector {
	
	public static final V2f[] circleToCircle (float theAX, float theAY, float theARadius, float theBX, float theBY, float theBRadius) {
		float d1 = (float) Math.sqrt (Math.pow (theBX - theAX, 2) + Math.pow (theBY - theAY, 2));
		
		if (d1 == 0 && theARadius == theBRadius || 
			d1 > theARadius + theBRadius || 
			d1 < Math.abs (theARadius - theBRadius)) {
			
			return null;
		}
		
		// First case - one intersection
		if (d1 == theARadius + theBRadius) {
			return new V2f[] {new V2f (	(theAX - theBX) / (theARadius + theBRadius) * theARadius + theAX, 
										(theAY - theBY) / (theARadius + theBRadius) * theARadius + theBY)
										};
		}
		
		// Second case - two intersections
		float cross = ((theARadius * theARadius) - (theBRadius * theBRadius) + (d1 * d1)) / (2f * d1);
		V2f v1		= new V2f	(theAX + ((theBX - theAX) * cross / d1), theAY + ((theBY - theAY) * cross / d1));
		float d2	= (float) Math.sqrt (theARadius * theARadius - cross * cross);
		V2f v2		= new V2f (-(theBY - theAY) * (d2 / d1), (theBX - theAX) * (d2 / d1));
		
		return new V2f[] {	V2f.sum (v1, v2), 
							V2f.sub (v1, v2)
							};
	}
	
	public static final V2f[] circleToCircle (V2f thePositionA, float theRadiusA, V2f thePositionB, float theRadiusB) {
		return circleToCircle (thePositionA.x, thePositionA.y, theRadiusA, thePositionB.x, thePositionB.y, theRadiusB);
	}
	
	public static final V2f[] circleToCircle (Circle theA, Circle theB) {	
		return circleToCircle (theA.position, theA.radius, theB.position, theB.radius);
	}
	
	public static final V2f[] circleToRay (float theCX, float theCY, float theRadius, float theAX, float theAY, float theBX, float theBY) {
		V2f v1 = new V2f (theBX - theAX, theBY - theAY);
		V2f v2 = new V2f (theCX - theAX, theCY - theAY);
		
		float dot = v1.dot (v2);
		
		V2f projl = new V2f (dot / (v1.lengthSquared ()) * v1.x, (dot / v1.lengthSquared ()) * v1.y);
		V2f midpt = new V2f (theAX + projl.x, theAY + projl.y);
		
		float disCenter = (midpt.x - theCX) * (midpt.x - theCX) + (midpt.y - theCY) * (midpt.y - theCY);
		
		if (disCenter > theRadius * theRadius) {
			return null;
		}
		if (disCenter == theRadius * theRadius) {
			return new V2f[] {midpt};
		}
		
		float disInter;
		if (disCenter == 0) {
			disInter = theRadius;
		}else{
			disCenter	= (float) Math.sqrt (disCenter);
			disInter	= (float) Math.sqrt (theRadius * theRadius - disCenter * disCenter);
		}
		
		float segment = 1f / v1.length ();
		
		v1.scale (segment);
		v1.scale (disInter);
		
		return new V2f[] {V2f.sum (midpt, v1), V2f.sub (midpt, v1)};
	}
	
	public static final V2f[] circleToRay (Circle theCircle, float theAX, float theAY, float theBX, float theBY) {
		return circleToRay (theCircle.position.x, theCircle.position.y, theCircle.radius, theAX, theAY, theBX, theBY);
	}
	
	public static final V2f[] circleToRay (float theCX, float theCY, float theRadius, Line2D theLine) {
		return circleToRay (theCX, theCY, theRadius, theLine.a.x, theLine.a.y, theLine.b.x, theLine.b.y);
	}
	
	public static final V2f[] circleToRay (Circle theCircle, Line2D theLine) {
		return circleToRay (theCircle.position.x, theCircle.position.y, theCircle.radius, theLine.a.x, theLine.a.y, theLine.b.x, theLine.b.y);
	}
	
	
	public static final V2f[] circleToLine (float theCX, float theCY, float theRadius, float theAX, float theAY, float theBX, float theBY) {
		V2f v1 = new V2f (theBX - theAX, theBY - theAY);
		V2f v2 = new V2f (theCX - theAX, theCY - theAY);
		
		float dot = v1.dot (v2);
		
		V2f projl = new V2f (dot / (v1.lengthSquared ()) * v1.x, (dot / v1.lengthSquared ()) * v1.y);
		V2f midpt = new V2f (theAX + projl.x, theAY + projl.y);
		
		float disCenter = (midpt.x - theCX) * (midpt.x - theCX) + (midpt.y - theCY) * (midpt.y - theCY);
		
		if (disCenter > theRadius * theRadius) {
			return null;
		}
		if (disCenter == theRadius * theRadius) {
			return new V2f[] {midpt};
		}
		
		float disInter;
		if (disCenter == 0) {
			disInter = theRadius;
		}else{
			disCenter	= (float) Math.sqrt (disCenter);
			disInter	= (float) Math.sqrt (theRadius * theRadius - disCenter * disCenter);
		}
		
		float segment = 1f / v1.length ();
		
		v1.scale (segment);
		v1.scale (disInter);
		
		V2f[] res = null;
		
		V2f i1 = V2f.sum (midpt, v1);
		V2f i2 = V2f.sub (midpt, v1);
		
		if ((i1.x - theAX) * v1.x + (i1.y - theAY) * v1.y > 0 && 
			(i1.x - theBX) * v1.x + (i1.y - theBY) * v1.y < 0) {
			
			res = new V2f[] {i1};
		}
		if ((i2.x - theAX) * v1.x + (i2.y - theAY) * v1.y > 0 && 
			(i2.x - theBX) * v1.x + (i2.y - theBY) * v1.y < 0) {
			
			if (res != null) {
				res = new V2f[] {i1, i2};
			}else{
				res = new V2f[] {i2};
			}
		}
		v1		= null;
		v2		= null;
		i1		= null;
		i2		= null;
		projl	= null;
		midpt	= null;
		
		return res;
	}
	
	public static final V2f[] circleToLine (Circle theCircle, float theAX, float theAY, float theBX, float theBY) {
		return circleToLine (theCircle.position.x, theCircle.position.y, theCircle.radius, theAX, theAY, theBX, theBY);
	}
	
	public static final V2f[] circleToLine (Circle theCircle, Line2D theLine) {
		return circleToLine (theCircle, theLine.a.x, theLine.a.y, theLine.b.x, theLine.b.y);
	}
	
	public static final V2f rayToRay (float theX1, float theY1, float theX2, float theY2, float theX3, float theY3, float theX4, float theY4) {
		float u = ((theY4 - theY3) * (theX2 - theX1) - (theX4 - theX3) * (theY2 - theY1));
		
		if (u != 0) {
			float ua = ((theX4 - theX3) * (theY1 - theY3) - (theY4 - theY3) * (theX1 - theX3)) / u;
			return new V2f (theX1 + ua * (theX2 - theX1), 
							theY1 + ua * (theY2 - theY1)
							);
		}
		return null;
	}
	
	public static final V2f rayToRay (V2f theAA, V2f theAB, V2f theBA, V2f theBB) {
		return rayToRay (theAA.x, theAA.y, theAB.x, theAB.y, theBA.x, theBA.y, theBB.x, theBA.y);
	}
	
	public static final V2f rayToRay (Line2D theA, Line2D theB) {
		return rayToRay (theA.a.x, theA.a.y, theA.b.x, theA.b.y, theB.a.x, theB.a.y, theB.b.x, theB.b.y);
	}
	
	public static final V2f rayToLine (float theX1, float theY1, float theX2, float theY2, float theX3, float theY3, float theX4, float theY4) {
		float u = ((theY4 - theY3) * (theX2 - theX1) - (theX4 - theX3) * (theY2 - theY1));
		
		if (u != 0) {
			float ua = ((theX4 - theX3) * (theY1 - theY3) - (theY4 - theY3) * (theX1 - theX3)) / u;
			if (ua >= 0 && ua <= 1) {
				return new V2f (theX1 + ua * (theX2 - theX1), 
								theY1 + ua * (theY2 - theY1)
								);
			}
		}
		return null;
	}
	
	public static final V2f rayToLine (V2f theAA, V2f theAB, V2f theBA, V2f theBB) {
		return rayToLine (theAA.x, theAA.y, theAB.x, theAB.y, theBA.x, theBA.y, theBB.x, theBB.y);
	}
	
	public static final V2f rayToLine (Line2D theA, Line2D theB) {
		return rayToLine (theA.a, theA.b, theB.a, theB.b);
	}
	
	public static final V2f lineToLine (float theX1, float theY1, float theX2, float theY2, float theX3, float theY3, float theX4, float theY4) {
		float u = ((theY4 - theY3) * (theX2 - theX1) - (theX4 - theX3) * (theY2 - theY1));
		
		if (u != 0) {
			float ua = ((theX4 - theX3) * (theY1 - theY3) - (theY4 - theY3) * (theX1 - theX3)) / u;
			float ub = ((theX2 - theX1) * (theY1 - theY3) - (theY2 - theY1) * (theX1 - theX3)) / u;
			
			if (ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1) {
				return new V2f (theX1 + ua * (theX2 - theX1), 
								theY1 + ua * (theY2 - theY1)
								);
			}
		}
		return null;
	}
	
	public static final V2f lineToLine (V2f theAA, V2f theAB, V2f theBA, V2f theBB) {
		return lineToLine (theAA.x, theAA.y, theAB.x, theAB.y, theBA.x, theBA.y, theBB.x, theBB.y);
	}
	
	public static final V2f lineToLine (Line2D theA, Line2D theB) {
		return lineToLine (theA.a, theA.b, theB.a, theB.b);
	}
	
	public static final V2fList splineToLine (Spline2D theSpline, V2f theA, V2f theB) {
		return splineToLine (theSpline, new Line2D (theA, theB));
	}
	
	public static final V2fList splineToLine (Spline2D theSpline, V2f theA, V2f theB, int theResolution) {
		return splineToLine (theSpline, new Line2D (theA, theB), theResolution);
	}
	
	public static final V2fList splineToLine (Spline2D theSpline, Line2D theLine) {		
		return splineToLine (theSpline, theLine, theSpline.points ().size ());
	}
	
	public static final V2fList splineToLine (Spline2D theSpline, Line2D theLine, int theResolution) {
		return splineLineSubdiv (	theSpline, 
									0, 
									theSpline.vertices ().length - 1, 
									theLine, 
									theResolution);
	}
	
	private static final V2fList splineLineSubdiv (Spline2D theSpline, int theStartVertex, int theEndVertex, Line2D theLine, int theSubDivisions) {
		
		int subdiv = theSubDivisions;
		int end;
		V2f v;
		V2fList res = new V2fList ();
		
		/* Test if number of requested subdivision parts 
		 * are perform able in context of the available 
		 * vertex segments. If not, redefine subdiv.
		 */
		if (subdiv > theEndVertex - theStartVertex) {
			subdiv = theEndVertex - theStartVertex;
		}
		
		/* Size of the steps between the single vertices 
		 * which will be connected to lines for intersection 
		 * testing.
		 */
		final int stepSize = (int) Math.floor ((theEndVertex - theStartVertex) / subdiv);
		
		// For subdivision resolution
		for (int i=0; i < subdiv; i++) {
			// Make sure the end vertex is included
			end = (i < subdiv - 1) ? theStartVertex + stepSize * (i + 1) : theEndVertex;
			
			// Subdivision <-> Line intersection?
			v = lineToLine (theLine.a, 
							theLine.b, 
							theSpline.vertices ()[theStartVertex + stepSize * i], 
							theSpline.vertices ()[end]
							);
			
			if (v != null) {
				/* Return the intersection point if the 
				 * lowest resolution level is reached 
				 * ("real" curve segment).
				 */
				if (end - theStartVertex == 1) {
					res.add (v);
				}else{
					// Continue to explode segments
					res.add (splineLineSubdiv (	theSpline, 
												theStartVertex + stepSize * i, 
												end, 
												theLine, 
												2
												));
				}
			}
		}
		v = null;
		
		return res;
	}
	
	public static final V2fList splineToSplineAsPoint (Spline2D theA, Spline2D theB) {
		if (theA.points ().size () < 2 || theB.points ().size () < 2) {
			return new V2fList ();
		}else{
			return splineSplinePointSubdiv (theA, 
											0, 
											theA.vertices ().length - 1, 
											theB, 
											0, 
											theB.vertices ().length - 1
											);
		}
	}
	
	public static final SplineSegment2D[] splineToSplineAsSegment (Spline2D theA, Spline2D theB) {
		if (theA.points ().size () < 2 || theB.points ().size () < 2) {
			return new SplineSegment2D[0];
		}else{
			return splineSplineSegmentSubdiv (	theA, 
												0, 
												theA.vertices ().length - 1, 
												theB, 
												0,
												theB.vertices ().length - 1);
		}
	}
	
	private static final SplineSegment2D[] splineSplineSegmentSubdiv (Spline2D theA, int theAStartVertex, int theAEndVertex, Spline2D theB, int theBStartVertex, int theBEndVertex) {
		SplineSegment2D[] res = new SplineSegment2D[0];
		V2f v;
		
		final int subdivfactor = 5;
		
		int enda;
		int endb;
		
		int subdiva = (theAStartVertex == 0 && theAEndVertex == theA.vertices ().length - 1) ? theA.points ().size () * subdivfactor : 2;
		int subdivb = (theBStartVertex == 0 && theBEndVertex == theB.vertices ().length - 1) ? theB.points ().size () * subdivfactor : 2;
		
		final int stepSizeA = (int) Math.floor ((theAEndVertex - theAStartVertex) / subdiva);
		final int stepSizeB = (int) Math.floor ((theBEndVertex - theBStartVertex) / subdivb);
		
		for (int a = 0; a < subdiva; a++) {
			enda = (a < subdiva - 1) ? theAStartVertex + stepSizeA * (a + 1) : theAEndVertex;
			for (int b = 0; b < subdivb; b++) {
				endb = (b < subdivb - 1) ? theBStartVertex + stepSizeB * (b + 1) : theBEndVertex;
				
				v = lineToLine (theA.vertices ()[theAStartVertex + stepSizeA * a], 
								theA.vertices ()[enda], 
								theB.vertices ()[theBStartVertex + stepSizeB * b], 
								theB.vertices ()[endb]
								);
				
				if (v != null) {
					
					SplineSegment2D[] tmp1;
					SplineSegment2D[] tmp2;
					
					if (enda - theAStartVertex == 1 && 
						endb - theBStartVertex == 1) {
						
						tmp1 = new SplineSegment2D[res.length];
						tmp2 = new SplineSegment2D[] {	new SplineSegment2D (theA, theA.vertices ()[theAStartVertex + stepSizeA * a], theA.vertices ()[enda]), 
														new SplineSegment2D (theB, theB.vertices ()[theBStartVertex + stepSizeB * b], theB.vertices ()[endb])
													};
						
						System.arraycopy (res, 0, tmp1, 0, res.length);
						
						res = new SplineSegment2D[tmp1.length + tmp2.length];
						
						System.arraycopy (tmp1, 0, res, 0, tmp1.length);
						System.arraycopy (tmp2, 0, res, tmp1.length, tmp2.length);
					}else{
						tmp1 = new SplineSegment2D[res.length];
						tmp2 = splineSplineSegmentSubdiv (	theA, 
															theAStartVertex + stepSizeA * a, 
															enda, 
															theB, 
															theBStartVertex + stepSizeB * b, 
															endb
															);
						System.arraycopy (res, 0, tmp1, 0, res.length);
						
						res = new SplineSegment2D[tmp1.length + tmp2.length];
						
						System.arraycopy (tmp1, 0, res, 0, tmp1.length);
						System.arraycopy (tmp2, 0, res, tmp1.length, tmp2.length);
					}
					
					tmp1 = null;
					tmp2 = null;
				}
				
				v = null;
			}
		}
		return res;
	}
	
	private static final V2fList splineSplinePointSubdiv (Spline2D theA, int theAStartVertex, int theAEndVertex, Spline2D theB, int theBStartVertex, int theBEndVertex) {
		
		V2fList res = new V2fList ();
		V2f v;
		
		final int subdivfactor = 5;
		
		int enda;
		int endb;
		
		int subdiva = (theAStartVertex == 0 && theAEndVertex == theA.vertices ().length - 1) ? theA.points ().size () * subdivfactor : 2;
		int subdivb = (theBStartVertex == 0 && theBEndVertex == theB.vertices ().length - 1) ? theB.points ().size () * subdivfactor : 2;
		
		final int stepSizeA = (int) Math.floor ((theAEndVertex - theAStartVertex) / subdiva);
		final int stepSizeB = (int) Math.floor ((theBEndVertex - theBStartVertex) / subdivb);
		
		for (int a = 0; a < subdiva; a++) {
			enda = (a < subdiva - 1) ? theAStartVertex + stepSizeA * (a + 1) : theAEndVertex;
			for (int b = 0; b < subdivb; b++) {
				endb = (b < subdivb - 1) ? theBStartVertex + stepSizeB * (b + 1) : theBEndVertex;
				
				v = lineToLine (theA.vertices ()[theAStartVertex + stepSizeA * a], 
								theA.vertices ()[enda], 
								theB.vertices ()[theBStartVertex + stepSizeB * b], 
								theB.vertices ()[endb]
								);
				
				if (v != null) {
					if (enda - theAStartVertex == 1 && 
						endb - theBStartVertex == 1) {
						
						res.add (v);
					}else{
						res.add (splineSplinePointSubdiv (	theA, 
															theAStartVertex + stepSizeA * a, 
															enda, 
															theB, 
															theBStartVertex + stepSizeB * b, 
															endb
														));
					}
				}
			}
		}
		v = null;
		
		return res;
	}
}